%initialze model
load('results\RoughGround\optimizedGains.mat')

BodyMechParams
ControlParams
prostheticParams

[groundX, groundZ, groundTheta] = generateGround('flat');

model = 'NeuromuscularModelwImpedance';
load_system(model)
rtp = Simulink.BlockDiagram.buildRapidAcceleratorTarget(model);

%set gains
rtp = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
    'LGainGAS',          Gains( 1), ...
    'LGainGLU',          Gains( 2), ...
    'LGainHAM',          Gains( 3), ...
    'LGainKneeOverExt',  Gains( 4), ...
    'LGainSOL',          Gains( 5), ...
    'LGainSOLTA',        Gains( 6), ...
    'LGainTA',           Gains( 7), ...
    'LGainVAS',          Gains( 8), ...
    'LKglu',             Gains( 9), ...
    'LPosGainGG',        Gains(10), ...
    'LSpeedGainGG',      Gains(11), ...
    'LhipDGain',         Gains(12), ...
    'LhipPGain',         Gains(13), ...
    'LkneeExtendGain',   Gains(14), ...
    'LkneeFlexGain',     Gains(15), ...
    'LkneeHoldGain1',    Gains(16), ...
    'LkneeHoldGain2',    Gains(17), ...
    'LkneeStopGain',     Gains(18), ...
    'LlegAngleFilter',   Gains(19), ...
    'RGainGLU',          Gains(20), ...
    'RGainHAM',          Gains(21), ...
    'RKglu',             Gains(22), ...
    'RPosGainGG',        Gains(23), ...
    'RSpeedGainGG',      Gains(24), ...
    'RhipDGain',         Gains(25), ...
    'RhipPGain',         Gains(26), ...
    'RlegAngleFilter',   Gains(27), ...
    'anklePgain',        Gains(28), ...
    'ankleDgain',        Gains(29), ...
    'ankleFilterPID',    Gains(30), ...
    'ankleFilterSEA',    Gains(31), ...
    'kneePgain',         Gains(32), ...
    'kneeDgain',         Gains(33), ...
    'kneeFilterPID',     Gains(34), ...
    'kneeFilterSEA',     Gains(35), ...
    'bKneeSwing1',       Gains(36), ...
    'bKneeSwing2',       Gains(37), ...
    'k1AnkleStance1',    Gains(38), ...
    'k1AnkleStance2',    Gains(39), ...
    'k1AnkleSwing1',     Gains(40), ...
    'k1AnkleSwing2',     Gains(41), ...
    'k1KneeStance1',     Gains(42), ...
    'k1KneeStance2',     Gains(43), ...
    'k1KneeSwing1',      Gains(44), ...
    'k1KneeSwing2',      Gains(45), ...
    'k2AnkleStance1',    Gains(46), ...
    'k2KneeStance2',     Gains(47), ...
    'ankleAngleThr',     Gains(48), ...
    'phiAnkleStance1',   Gains(49), ...
    'phiAnkleStance12',  Gains(50), ...
    'phiAnkleStance2',   Gains(51), ...
    'phiKneeStance1',    Gains(52), ...
    'phiKneeStance2',    Gains(53), ...
    'phiKneeSwing1',     Gains(54), ...
    'phiKneeSwing2',     Gains(55), ...
    'legLengthClr',      Gains(56), ...
    'simbiconGainD',     Gains(57), ...
    'simbiconGainV',     Gains(58), ...
    'simbiconLegAngle0', Gains(59));

numSamples = 50;
terrains = 0:0.02:.14;
numTerrains = length(terrains);
totalRuns = numSamples*numTerrains;
dists = nan(totalRuns,1);
paramSets = cell(1,totalRuns);
idx = 1;
for i = 1:numTerrains
    for j = 1:numSamples
        [groundX, groundZ, groundTheta] = generateGround('const',terrains(i),idx);

        paramSets{idx} = Simulink.BlockDiagram.modifyTunableParameters(rtp, ...
            'groundZ',     groundZ, ...
            'groundTheta', groundTheta);
        idx = idx+1;
    end
end

parfor i = 1:length(paramSets)
    dists(i) = evaluateCostParallel(paramSets{i});
end

dists = reshape(dists,numSamples,[])
meandists = nanmean(dists)

save('distances.mat','dists')
